#include "PrecompiledHeaders.h"
#include "PodServerInterface.h"
#include "Utilities.h"

// Joy stick values
#define X             0
#define Y             1
#define THUMBBUTT     22
#define TRIGGER       23
#define JOYMINBOX_X   65
#define JOYMINBOX_Y   65
#define JOYMAXBOX_X   220
#define JOYMAXBOX_Y   220
#define JOYMAXVALUE_X 300
#define JOYMAXVALUE_Y 300

double PodServerInterface::last = getCurrentTime();
bool PodServerInterface::poll = true;
bool PodServerInterface::move = true;
bool PodServerInterface::moved = false;
bool PodServerInterface::poll_connected = false;
bool PodServerInterface::move_connected = false;
SocketClient *PodServerInterface::moveClient = new SocketClient();
SocketClient *PodServerInterface::pollClient = new SocketClient();
polldata PodServerInterface::pdata;
movedata PodServerInterface::mdata;
float PodServerInterface::pitch_speed = 0;
float PodServerInterface::roll_speed = 0;
std::vector<pod_position> PodServerInterface::pod_positions(0);
int PodServerInterface::index;
float **PodServerInterface::joySmooth = NULL;
float PodServerInterface::joyXtotal;
float PodServerInterface::joyYtotal;
float PodServerInterface::joyX;
float PodServerInterface::joyY;
float PodServerInterface::thumbButt;
float PodServerInterface::trigger;
int PodServerInterface::pollCount = 0;
int PodServerInterface::moveCount = 0;

float PodServerInterface::POD_MOVEMENT_DELAY = 0.28;
float PodServerInterface::MAX_BALL_ACCELERATION = 80;
float PodServerInterface::P_OR_R_SPEED_CHANGE = 0.05;
float PodServerInterface::P_OR_R_TIME = 3;

/*
 * Default constructor
 */
PodServerInterface::PodServerInterface(void)
{
    // initialise server connections
    //delete pollClient;
    //delete moveClient;
    commandClient = new SocketClient(ADDRESS,tcp,TCP_PORT);
    moveClient = new SocketClient(ADDRESS,tcp,TCP_PORT);
    pollClient = new SocketClient(ADDRESS,tcp,TCP_PORT);
    command_connected = commandClient->isConnected();
    poll_connected = pollClient->isConnected();
    move_connected = moveClient->isConnected();
    
    // initialise joysick smoothing chache
    joySmooth = (float **)calloc(2, sizeof(float *));//new float *[2];
    joySmooth[0] = (float *)calloc(SMOOTH_SIZE, sizeof(float));
    joySmooth[1] = (float *)calloc(SMOOTH_SIZE, sizeof(float));
    joyXtotal=0;
    joyYtotal=0;
    index = 0;
    
    // initialise threads to communicate with server
    if(move_connected) moveClient->sendUnsignedInt(moveClient, (unsigned int)cmdSTARTMOVING);
    if(poll_connected) pollClient->sendUnsignedInt(pollClient, (unsigned int)cmdSTARTPOLLING);
    Thread m(&pollLoop);
    Thread p(&moveLoop);
    if(command_connected) commandClient->sendUnsignedInt(commandClient, (unsigned int) cmdSTART);
}

/*
 * Destructor
 */
PodServerInterface::~PodServerInterface(void)
{
    delete commandClient;
    delete moveClient;
    delete pollClient;
    delete[] joySmooth[0];
    delete[] joySmooth[1];
    delete[] joySmooth;
}

/*
 * Copy constructor
 */
PodServerInterface::PodServerInterface(const PodServerInterface& o)
{
    commandClient = new SocketClient(ADDRESS,tcp,TCP_PORT);
}

/*
 * Equality operator override
 */
PodServerInterface& PodServerInterface::operator=(PodServerInterface& o)
{
   commandClient = o.commandClient;
   moveClient = o.moveClient;
   pollClient = o.pollClient;
   return *this;
}

/*
 * Send the pod to home position
 */ 
void PodServerInterface::home()
{
    pod_positions.clear();
    if(!command_connected)
        return;
    commandClient->sendUnsignedInt(commandClient, (unsigned int) cmdHOME);
}

/*
 * Stop communication with pod server
 */
void PodServerInterface::stop()
{
    home();
    wait(1.0);
    // kill threads
    poll = false;
    move = false;
    
    // wait untill threads have terminated
    while(poll_connected || move_connected) { wait(0.001); }
    
    // send end messages to server and kill command client
    if(command_connected) {
        commandClient->sendUnsignedInt(commandClient, (unsigned int) cmdSTOPPOLLING);
        commandClient->sendUnsignedInt(commandClient, (unsigned int) cmdSTOPMOVING);
        commandClient->sendUnsignedInt(commandClient, (unsigned int) cmdHOME);
        commandClient->sendUnsignedInt(commandClient, (unsigned int) cmdSTOP);
        commandClient->disconnect();
    }
}

/*
 * Add a new acceleration to a vector of those to be simulated
 */
void PodServerInterface::simulateAcceleration(Ogre::Vector3 acceleration)
{
    if(acceleration.length() > MAX_BALL_ACCELERATION) {
        acceleration *= MAX_BALL_ACCELERATION / acceleration.length();
    }
    pod_position now;
    now.acceleration = acceleration;
    now.time_ago = 0;
    pod_positions.push_back(now);
}

/*
 * Update the values of roll and pitch
 */
void PodServerInterface::setRollAndPitch(float r, float p)
{
    float pitch_needed = p - mdata.p, roll_needed = r - mdata.r;
    if(pitch_needed > pitch_speed * P_OR_R_TIME) pitch_speed += P_OR_R_SPEED_CHANGE;
    else pitch_speed -= P_OR_R_SPEED_CHANGE;
    mdata.p += pitch_speed * 0.05;
    if(roll_needed > roll_speed * P_OR_R_TIME) roll_speed += P_OR_R_SPEED_CHANGE;
    else roll_speed -= P_OR_R_SPEED_CHANGE;
    mdata.r += roll_speed * 0.05;
}

/*
 * Poll inputs to update the input value array, should be called on each
 * iteration of the game loop.
 */
void PodServerInterface::pollInputs()
{
    // prevent the pollLoop thread from hogging the processor
    if(!poll_connected) {
        wait(0.001);
        return;
    }
    
    // iterate the value of index
    if(index >= SMOOTH_SIZE) index -= SMOOTH_SIZE;
    
    // subtract oldest value
    joyXtotal -= joySmooth[X][index];
    joyYtotal -= joySmooth[Y][index];
    
    // receive new poll data
    int r = pollClient->receiveStruct(pollClient, &pdata, sizeof(polldata));
    if(r <= 0) {
        pollClient->receiveError(r, pollClient);
    }
    
    //printf("\rx: %6i y: %6i", pdata.inputs[X], pdata.inputs[Y]);
         
    // update new values and calculate average
    //joySmooth[X][index] = (float)pdata.inputs[X];
    //joySmooth[Y][index] = (float)pdata.inputs[Y];
    /*joySmooth[X][index] = (float)processJoysickXValue(pdata.inputs[X]);
    joySmooth[Y][index] = (float)processJoysickYValue(pdata.inputs[Y]);
    joyXtotal += joySmooth[X][index];
    joyYtotal += joySmooth[Y][index];
    joyX = joyXtotal / (float)SMOOTH_SIZE;
    joyY = joyYtotal / (float)SMOOTH_SIZE;
    thumbButt = (float)pdata.inputs[THUMBBUTT];
    trigger = (float)pdata.inputs[TRIGGER];*/
    index++;
}

int PodServerInterface::processJoysickXValue(int x)
{
    if(x < JOYMINBOX_X && x > -JOYMINBOX_X) return 0;
    if(x >  JOYMAXBOX_X) return  JOYMAXVALUE_X;
    if(x < -JOYMAXBOX_X) return -JOYMAXVALUE_X;
    return x;
}

int PodServerInterface::processJoysickYValue(int y)
{
    if(y < JOYMINBOX_Y && y > -JOYMINBOX_Y) return 0;
    if(y >  JOYMAXBOX_Y) return  JOYMAXVALUE_Y;
    if(y < -JOYMAXBOX_Y) return -JOYMAXVALUE_Y;
    return y;
}

/*
 * Send the pod server the next position of the pod
 */
void PodServerInterface::sendMove()
{
    if(!move_connected) return;
    
    double now = getCurrentTime();
    double time_increment = now - last;
    float factor, x = 0, y = 0, z = 0;
    last = now;
    
    // update the pod position from a set of previous decaying values
    for(unsigned int i = 0; i < pod_positions.size(); i++)
    {
        factor = (1 + sin((pod_positions[i].time_ago / POD_MOVEMENT_DECAY) + 1)) / 2;
        if(factor < 0.05) {
            pod_positions.erase(pod_positions.begin() + i);
            i--;
            continue;
        }
        //printf("Factor at time %f = %f\n", pod_positions[i].time_ago, factor);
        x += pod_positions[i].acceleration.x * factor;
        y += pod_positions[i].acceleration.y * factor;
        z += pod_positions[i].acceleration.z * factor;
        pod_positions[i].time_ago += time_increment;
    }
    
    // Normalise the values
    x *= MAX_MOVEMENT / MAX_BALL_ACCELERATION;
    y *= MAX_MOVEMENT / MAX_BALL_ACCELERATION;
    z *= MAX_MOVEMENT / MAX_BALL_ACCELERATION;
    
    //printf("\rPod position (x y z in Ogre): %.2f %.2f %.2f", x, y, z);
    // in ogre: x = left/right, y = up/down     z = back/front
    // on pod:  x = front/back, y = left/right, z = up/down

    mdata.x = constrainMovement(-z);
    mdata.y = constrainMovement(x);
    mdata.z = constrainMovement(y);
    
    moveClient->sendStruct(moveClient, &mdata, sizeof(movedata));
}

/*
 * Cap a movement value against +/- thresholds
 */
float PodServerInterface::constrainMovement(float value)
{
    if(value >  MAX_MOVEMENT) return (float)MAX_MOVEMENT;
    else if(value < -MAX_MOVEMENT) return (float)-MAX_MOVEMENT;
    return value;
}

/*
 * Thread method to send move commands
 */
void PodServerInterface::moveLoop(void *a)
{
    clock_t start = clock(), goal;
    double interval = MOVE_INTERVAL * (double)CLOCKS_PER_SEC;
    while(move){
        sendMove();
        moveCount++;
        //printf("\rSent move %6i", moveCount);
        goal = start + (clock_t)interval;
        while(clock() < goal) wait(0.0001);
        start = clock();
        //wait(Constants::MOVE_INTERVAL);
    }
    if(move_connected) {
        moveClient->disconnect();
        move_connected = false;
    }
}

/*
 * Thread method to receive poll data
 */
void PodServerInterface::pollLoop(void *a)
{
    while(poll){
        pollInputs();
        //printf("\r[%8i]x: %10f (%6i) y: %10f (%6i)", 
            //pdata.inputs[15], joyX, pdata.inputs[X], joyY, pdata.inputs[Y]);
    }
    if(poll_connected) {
        pollClient->disconnect();
        poll_connected = false;
    }
}

/*
 * Return the value of the joystick x axis
 */
float PodServerInterface::getJoystickX()
{
    return joyX;
}

/*
 * Return the value of the joystick y axis
 */ 
float PodServerInterface::getJoystickY()
{
    return joyY;
}

/*
 * Get the thumb button as a boolean value
 */
bool PodServerInterface::getThumbButt()
{
    return thumbButt > 0;
}

/*
 * Get the trigger button as a boolean value
 */
bool PodServerInterface::getTriggerButt()
{
    return trigger > 0;
}
